# Motion Interface for soul planner

# How to call a move
# you import the motioninterface as mot
# mot.move("normalPose")
# mot.move("rKickAngled", 0.5)
# mot.move("walkTo", (1,0,0))

import time
import motions_IR_v3 as motion

# METHOD FOR MOTIONS
def changeHead(Arg):
	motion.changeHead(Arg)

def setHead(Arg):
	motion.setHead(Arg)

def normalPose():
	motion.normalPose()

def normalHead():
	motion.normalHead()
	
def walkTo(Arg):
	motion.walkTo(Arg)

def postWalkTo(Arg):
	motion.postWalkTo(Arg)	

def rKickAngled(Arg):
	motion.rKickAngled(Arg)

def lKickAngled(Arg):
	motion.lKickAngled(Arg)

def hakje():
	motion.hakje()

def footLeft():
	motion.footLeft()

def footRight():
	motion.footRight()

def diveLeft():
	motion.diveLeft()

def diveRight():
	motion.diveRight()

def stance():
	motion.stance()	
	
moves =	{
	"setHead": setHead,
	"changeHead": changeHead,
	"normalPose": normalPose,
	"normalHead": normalHead,
	"postWalkTo": postWalkTo,
	"walkTo": walkTo,
	"rKickAngled": rKickAngled,
	"lKickAngled": lKickAngled,
	"hakje": hakje,
	"footLeft": footLeft,
	"footRight": footRight,
	"diveLeft": diveLeft,
	"diveRight": diveRight,
	"stance": stance
}

# MOVES
def standUp():
	pose = motion.getPose()
	if pose == "Back":
		motion.stopTracker()
		motion.stiff()
		motion.backToStand()
		motion.startTracker()
		return True
	elif pose == "Belly":
		motion.stopTracker()
		motion.stiff()
		motion.bellyToStand()
		motion.startTracker()
		return True
	return False

def move(Type, Arg = None):
	# Voer movement uit
	if Arg == None:
		moves.get(Type)()
	else:
		moves.get(Type)(Arg)

def scanCircle(yaw):
	return motion.scanMove(yaw)
		
def stiff():
	motion.stiff()

def killWalk():
	motion.killWalk()

def getHead():
	return motion.getHead()

def setWTV(x,y,t,f):
	motion.setWTV(x,y,t,f)
		
	
# BOOLEANS
def isWalking():
	return motion.isWalking()
	
def getStance():
	return motion.getStance()

def getNormalPose():
	return motion.getNormalPose()

def killAllTasks():
	motion.killAllTasks()